import json
import os
import socket

from PyQt6.QtCore import QMetaObject, Qt, QThread, pyqtSignal
from PyQt6.QtWidgets import QMessageBox
import socket
import struct
import VTK成像
import 机械臂正逆解
import numpy as np
import threading
from threading import Event
import time
import traceback


RotaionMatrix = np.full((3, 3), None)
ZeroPoint = np.array([])
move_data = []
database = []
AxisAngle = []
stop_event = Event()  # 全局线程控制变量
host = "127.0.0.1"
port = 65432
DataBaseConfirm = False


def ChangeToFloat(textedit):
    text = textedit.toPlainText()
    if text == "":  # 判断是否为空
        # QMessageBox.warning(None, "输入错误", f"第 {index + 1} 个输入框未填写！")
        return None
    return float(text)  # 转为浮点数保存


def ChangeToByteStream0(List, ctrl=0):
    judge = 0
    byte_stream = struct.pack('i', judge) + b''.join(struct.pack('f', value) for value in List)
    return byte_stream


def ChangeToByteStream2(List, ctrl=0):
    judge = 2
    byte_stream = struct.pack('i', judge) + b''.join(struct.pack('f', value) for value in List)
    return byte_stream


def show_warning_message(main_window, message):
    """
    显示警告信息，用这种方式是为了防止在线程中弹出警告框，导致程序卡死
    :param message:
    :return:
    """
    main_window.warning_signal.emit(message)


def update_gui_safe(obj, method_name, *args):
    """在主线程安全更新 GUI 的工具函数"""
    QMetaObject.invokeMethod(obj, method_name, Qt.ConnectionType.QueuedConnection, *args)


def setOtherUiBotton(mainwindow, bool):
    """
    1、将线程运行过程中，会影响功能的按钮禁用
    2、在线程结束后，将其他按钮恢复
    :param mainwindow: 窗口
    :param bool: bool类型
    :return:
    """
    mainwindow.ReadFile.setEnabled(bool)
    mainwindow.CreateDBFile.setEnabled(bool)
    mainwindow.CreateModule.setEnabled(bool)
    mainwindow.CreatePoint.setEnabled(bool)
    mainwindow.SetPosition.setEnabled(bool)
    mainwindow.RoboMove.setEnabled(bool)
    mainwindow.CalculateMatrix.setEnabled(bool)
    mainwindow.StartConnect.setEnabled(bool)
    """
    一旦点下按钮后，不管成功还是失败都要禁用按钮，防止重复点击
    能够控制按钮的状态，只有通过测试才行
    """
    mainwindow.RoboMove.setEnabled(False)


def rotationmatrix(MainWindow):
    """
    计算旋转矩阵和模型零点在真实空间的位置
    """
    raw_point = VTK成像.fourpos.copy()
    P1 = np.array(raw_point[0])
    P2 = np.array(raw_point[1])
    P3 = np.array(raw_point[2])
    P4 = np.array(raw_point[3])
    # print(f"P1 = \n{P1}\nP2 = \n{P2}\nP3 = \n{P3}\nP4 = \n{P4}")
    # 计算三基的向量
    PVec1 = P2 - P1
    PVec2 = P3 - P1
    PVec3 = P4 - P1
    orgin_point = np.array([0, 0, 0])
    # 形成矩阵 A 和向量 B
    A = np.column_stack((PVec1, PVec2, PVec3))  # 3x3 矩阵
    B = orgin_point - P1  # 偏移向量
    print(f"A = \n{A}\nB = \n{B}")
    rank = np.linalg.matrix_rank(A)
    if rank != 3:
        QMessageBox.warning(None, "警告", "四点共面，无法求解")
        return
    else:
        # 求解 c1, c2, c3
        C = np.linalg.solve(A, B)  # 求解线性方程组 A * C = B
        print(f"C= \n{C}")
    robdata = []
    try:
        robdata.append(ChangeToFloat(MainWindow.Rob1A1))
        robdata.append(ChangeToFloat(MainWindow.Rob1A2))
        robdata.append(ChangeToFloat(MainWindow.Rob1A3))
        robdata.append(ChangeToFloat(MainWindow.Rob1A4))
        robdata.append(ChangeToFloat(MainWindow.Rob1A5))
        robdata.append(ChangeToFloat(MainWindow.Rob1A6))
        robdata.append(ChangeToFloat(MainWindow.Rob2A1))
        robdata.append(ChangeToFloat(MainWindow.Rob2A2))
        robdata.append(ChangeToFloat(MainWindow.Rob2A3))
        robdata.append(ChangeToFloat(MainWindow.Rob2A4))
        robdata.append(ChangeToFloat(MainWindow.Rob2A5))
        robdata.append(ChangeToFloat(MainWindow.Rob2A6))
        robdata.append(ChangeToFloat(MainWindow.Rob3A1))
        robdata.append(ChangeToFloat(MainWindow.Rob3A2))
        robdata.append(ChangeToFloat(MainWindow.Rob3A3))
        robdata.append(ChangeToFloat(MainWindow.Rob3A4))
        robdata.append(ChangeToFloat(MainWindow.Rob3A5))
        robdata.append(ChangeToFloat(MainWindow.Rob3A6))
        robdata.append(ChangeToFloat(MainWindow.Rob4A1))
        robdata.append(ChangeToFloat(MainWindow.Rob4A2))
        robdata.append(ChangeToFloat(MainWindow.Rob4A3))
        robdata.append(ChangeToFloat(MainWindow.Rob4A4))
        robdata.append(ChangeToFloat(MainWindow.Rob4A5))
        robdata.append(ChangeToFloat(MainWindow.Rob4A6))
    except Exception as e:
        print(f"错误: {e}")
    for i in robdata:
        if i is None:
            QMessageBox.warning(None, "警告", "请输入所有参数")
            return
    rob_1 = np.array(机械臂正逆解.forward_kinematics(robdata[0], robdata[1], robdata[2], robdata[3], robdata[4], robdata[5]))
    rob_2 = np.array(
        机械臂正逆解.forward_kinematics(robdata[6], robdata[7], robdata[8], robdata[9], robdata[10], robdata[11]))
    rob_3 = np.array(
        机械臂正逆解.forward_kinematics(robdata[12], robdata[13], robdata[14], robdata[15], robdata[16], robdata[17]))
    rob_4 = np.array(
        机械臂正逆解.forward_kinematics(robdata[18], robdata[19], robdata[20], robdata[21], robdata[22], robdata[23]))
    # print(f"rob_1 = \n{rob_1}\nrob_2 = \n{rob_2}\nrob_3 = \n{rob_3}\nrob_4 = \n{rob_4}")
    # 计算三基的向量
    RobVec1 = rob_2 - rob_1
    RobVec2 = rob_3 - rob_1
    RobVec3 = rob_4 - rob_1
    R1 = rob_1 + C[0] * RobVec1 + C[1] * RobVec2 + C[2] * RobVec3  # R1 代表旋转后的坐标系原点
    print(f"R1 = \n{R1}")
    # 计算旋转矩阵
    rob_2_new = rob_2 - R1
    rob_3_new = rob_3 - R1
    rob_4_new = rob_4 - R1
    AfterTrans = np.column_stack((rob_2_new, rob_3_new, rob_4_new))  # 现实中的三个不共面的坐标轴组成的3x3矩阵
    BeforeTrans = np.column_stack((P2, P3, P4))  # vtk空间中的三个不共面的坐标轴组成的3x3矩阵
    R = np.dot(AfterTrans, np.linalg.inv(BeforeTrans))  # R 代表旋转矩阵
    if np.linalg.det(R) < 0.9 or np.linalg.det(R) > 1.1:
        QMessageBox.warning(None, "警告", "旋转矩阵不正交，请检查参数")
        return
    global RotaionMatrix
    global ZeroPoint
    RotaionMatrix = R
    print(f"RotaionMatrix=\n{RotaionMatrix}")
    ZeroPoint = R1
    MainWindow.CreatePointDB.setEnabled(True)


class ServerThread(QThread):
    received_signal = pyqtSignal(str)
    close_signal = pyqtSignal()
    RoboCtrl_signal = pyqtSignal(bool)
    now_pos_signal = pyqtSignal(list)

    def __init__(self, module=2, host='172.31.1.100', port=59152):
        super().__init__()
        self.module = module
        self.host = host
        self.port = port
        self.running = False
        self.connection = None
        self.server_socket = None  # 监听套接字

    def run(self):

        try:
            # 创建并绑定服务端套接字
            with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as server_socket:
                self.server_socket = server_socket
                server_socket.settimeout(1)  # 设置超时时间，防止永久阻塞
                server_socket.bind((self.host, self.port))
                server_socket.listen()
                self.received_signal.emit("服务器配置完成, 等待连接...")
                self.running = True
                while self.running:
                    try:
                        if self.module == 2:
                            self.connection, addr = server_socket.accept()
                            self.received_signal.emit(f"成功连接客户端: {addr}")
                            time.sleep(1)
                            test1 = 机械臂正逆解.inverse_kinematics(400, 0, 600, 0, 0, 1)
                            test = []
                            for t in test1:
                                test.append(t)
                            print(test)
                            # self.send_message([1,1,1,1,1,1])
                            self.send_message(ChangeToByteStream2(test))
                            self.handle_client(self.connection)
                        elif self.module == 0:
                            self.connection, addr = server_socket.accept()
                            self.received_signal.emit(f"成功连接客户端: {addr}")
                            self.handle_client_2(self.connection)
                    except socket.timeout:
                        # 超时后检查是否需要继续运行
                        continue
                    except Exception as e:
                        # self.received_signal.emit(f"Error during client handling: {e}")
                        break
        except Exception as e:
            self.received_signal.emit(f"Server error: {e}")
        finally:
            self.running = False
            self.server_socket = None  # 清理监听套接字
            self.received_signal.emit("服务停止.")

    def handle_client(self, connection):
        try:
            while self.running:
                data = connection.recv(1024)
                if len(data) == 28:
                    unpacked = struct.unpack("i6f", data)
                    self.received_signal.emit(f"接收: 信号={unpacked[0]}\n 轴角度为={unpacked[1:]}")
                    self.received_signal.emit("测试成功,启用机械臂控制")
                    self.RoboCtrl_signal.emit(True)
                else:
                    self.received_signal.emit("字节数不为28，测试失败")
                    self.received_signal.emit("禁用机械臂控制")
                    self.RoboCtrl_signal.emit(False)
                break
        except Exception as e:
            self.received_signal.emit(f"错误: {e}")
        finally:
            connection.close()
            self.received_signal.emit("停止网络连接！")
            self.close_signal.emit()

    def handle_client_2(self, connection):
        try:
            global database, DataBaseConfirm
            if not DataBaseConfirm:
                self.received_signal.emit("请先创建点位数据库！")
                return
            Number = len(database)

            j=0
            for i in range(len(database)):
                time.sleep(0.2)
                self.send_message(ChangeToByteStream0(database[i]["AxisAngle"]))
                while self.running:
                    data = connection.recv(1024)
                    if len(data) == 28:
                        unpacked = struct.unpack("i6f", data)
                        # self.received_signal.emit(f"接收: 信号={unpacked[0]}\n 轴角度为={unpacked[1:]}")
                        self.received_signal.emit(f"已经移动至第{i + 1}个点，一共{Number}个点")
                        # a1,a2,a3,a4,a5,a6 = unpacked[1:]
                        # x, y, z ,rx, ry, rz = 机械臂正逆解.forward_kinematics(a1, a2, a3, a4, a5, a6)
                        # listpos = [x, y, z, rx, ry, rz]
                        # self.now_pos_signal.emit(listpos)
                        self.RoboCtrl_signal.emit(True)
                        j=j+1
                        break
                    else:
                        self.received_signal.emit(f"字节数为{len(data)},不为28，测试失败")
                        self.received_signal.emit("禁用机械臂控制")
                        self.RoboCtrl_signal.emit(False)
                        break
        except Exception as e:
            self.received_signal.emit(f"错误: {e}")
        finally:
            if j==Number:
                self.received_signal.emit("全部点位移动完成！")
            else:
                self.received_signal.emit(f"移动失败！已移动{j}个点位")
            connection.close()
            connection.close()
            self.received_signal.emit("停止网络连接！")
            self.close_signal.emit()

    def send_message(self, message=b"TestTestTestTestTestTestTest"):
        if self.connection:
            try:
                self.connection.sendall(message)
            except Exception as e:
                self.received_signal.emit(f"Error sending message: {e}")

    def stop(self):
        self.running = False
        if self.connection:
            try:
                self.connection.close()
            except Exception:
                pass  # 忽略关闭时的错误
        if self.server_socket:
            try:
                self.server_socket.close()  # 关闭监听套接字，打破阻塞状态
            except Exception:
                pass
        # self.received_signal.emit("Server stopped1.")


# def ListenSocket(MainWindow):
#     """
#     哥们麻了，这段代码应该是有空指针或者是其他错误，我也不知道怎么修复QAQ
#     :param MainWindow:
#     :return:
#     """
#     try:
#         global host, port, stop_event
#         tcp_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
#         MainWindow.RoboMove.setEnabled(False)  # 禁用开始链接按钮
#         try:
#             # 尝试绑定端口，如果失败，弹出警告，并且禁用按钮机械臂移动，然后恢复开始链接按钮
#             tcp_socket.bind((host, port))
#             MainWindow.TCPInfo.appendPlainText(f"成功配置ip: {host}和端口{port}")
#         except Exception as e:
#             show_warning_message(MainWindow, f"配置错误：{e} 发生！")
#             MainWindow.RoboMove.setEnabled(False)
#             stop_event.set()  # 停止线程
#             MainWindow.StartConnect.setEnabled(True)
#             return
#
#         if not stop_event.is_set():
#             try:
#                 # 如果链接成功，进入下一步try接受信号
#                 tcp_socket.listen(1)
#                 conn, addr = tcp_socket.accept()  # 阻塞等待连接
#                 if stop_event.is_set():
#                     return  # 如果接收到停止信号，退出循环，并且关闭套接字
#
#                 MainWindow.TCPInfo.appendPlainText(f"客户端已连接: {addr}")
#
#                 try:
#                     # 发送测试响应，如果失败会抛出异常
#                     response_signal = 2
#                     response_data = struct.pack("i6f", response_signal, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0)
#                     conn.sendall(response_data)
#                     MainWindow.TCPInfo.appendPlainText("测试响应数据已发送。")
#
#                     # 接收测试信号
#                     conn.setblocking(False)
#                     while not stop_event.is_set():
#                         try:
#                             data = conn.recv(28)
#                             if data:
#                                 break  # 收到数据后退出
#                         except BlockingIOError:
#                             continue  # 没有数据，继续检查
#                     if stop_event.is_set():
#                         return
#                     if len(data) < 28:
#                         show_warning_message(MainWindow, "数据长度错误")
#                         return
#                     else:
#                         MainWindow.RoboMove.setEnabled(True)
#                         MainWindow.TCPInfo.appendPlainText("测试连接成功")
#                         return
#                 except Exception as e:
#                     show_warning_message(MainWindow, f"接收或发送数据时出错: {e}")
#                     return
#                 finally:
#                     print("第一个finally执行完毕")
#                     conn.close()
#             except Exception as e:
#                 show_warning_message(MainWindow, f"连接错误：{e} 发生！")
#                 return
#             finally:
#                 print("第2个finally执行完毕")
#                 MainWindow.StartConnect.setEnabled(True)
#                 try:
#                     if conn:
#                         conn.close()  # 确保连接关闭
#                         print("conn关闭")
#                     if tcp_socket:
#                         tcp_socket.close()  # 确保套接字关闭
#                         print("tcp_socket关闭")
#                 except Exception as e:
#                     print(f"资源释放错误：{e}")
#         else:
#             # 线程安全退出
#             MainWindow.StartConnect.setEnabled(True)
#             if tcp_socket:
#                 tcp_socket.close()
#     except Exception as e:
#         print(f"连接错误：{e} 发生！")

# def StartListen(MainWindow):
#     try:
#         global stop_event
#         stop_event.clear()
#         # 创建线程，并且让开始链接按钮不可用
#         t=threading.Thread(target=ListenSocket,args=(MainWindow,))
#         MainWindow.StartConnect.setEnabled(False)
#         t.start()
#     except Exception as e:
#         print(f"连接错误：{e} 发生！")

# def StopListen(MainWindow):
#     global host, port, stop_event
#     stop_event.set()  # 通知线程停止
#
#     # 创建虚拟连接以解除 accept 阻塞,简单的说就是创建了一个客户端连接，然后出发线程后面定义的if stop_event.is_set():语句，进而实现线程的停止
#     try:
#         dummy_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
#         dummy_socket.connect((host, port))
#         dummy_socket.sendall(b"STOPSTOPSTOPSTOPSTOPSTOPSTOPSTOPSTOPSTOPSTOPSTOPSTOPSTOPSTOPSTOPSTOPSTOPSTOPSTOPSTOPSTOPSTOPSTOP")
#         dummy_socket.close()
#     except Exception:
#         pass
#
#     MainWindow.RoboMove.setEnabled(False)
#     MainWindow.TCPInfo.appendPlainText("连接中断！")


def make_point_db(MainWindow):
    """
    判断TCP是否连接成功，如果连接成功，则发送机械臂运动指令
    """

    # 进行数据运算
    # setOtherUiBotton(MainWindow, False)
    global RotaionMatrix
    global ZeroPoint
    global move_data
    global database
    global AxisAngle
    if RotaionMatrix.all() is None:
        QMessageBox.warning(None, "警告", "请先计算旋转矩阵")
        # setOtherUiBotton(MainWindow, True)
        return
    if ZeroPoint.all() is None:
        QMessageBox.warning(None, "警告", "请先计算零点")
        # setOtherUiBotton(MainWindow, True)
        return
    move_data = []
    database = []
    AxisAngle = []
    move_data1 = VTK成像.point_list.copy()
    move_data2 = VTK成像.point_normal_list.copy()
    for i in range(len(move_data1)):
        move_data1[i] = np.dot(RotaionMatrix, move_data1[i]) + ZeroPoint
        move_data2[i] = np.dot(RotaionMatrix, move_data2[i])
    move_data = np.hstack((move_data1, move_data2)).tolist()
    dataerror = False
    for i in range(len(move_data)):
        # 提取当前的点和法向量
        position = move_data[i][:3]
        normal = move_data[i][3:]

        # 解构 position 和 normal，用于传递给 `inverse_kinematics`
        x, y, z = position
        px, py, pz = normal

        # 调用 `inverse_kinematics` 并获取结果
        try:
            # 将结果添加到 AxisAngle 列表
            AxisAngle.clear()
            Return = 机械臂正逆解.inverse_kinematics(x, y, z, px, py, pz)
            for r in Return:
                AxisAngle.append(r)
        except ValueError as e:
            print(f"Error computing angles for index {i}: {e}")
            dataerror = True
            break

        # 构造字典并添加到 database
        data_entry = {
            "Position": position,
            "Normal": normal,
            "AxisAngle": AxisAngle.copy(),
            "peak": 0,
            "depth": 0
        }
        database.append(data_entry)

    global DataBaseConfirm
    if dataerror:
        show_warning_message(MainWindow, "数据有误，请检查")
        # setOtherUiBotton(MainWindow, True)
        DataBaseConfirm = False
        return
    else:
        MainWindow.TCPInfo.appendPlainText("数据计算成功！")
        DataBaseConfirm = True


